#ifndef MY_PCL_H
#define MY_PCL_H

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/common/pca.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Header.h>
#include <geometry_msgs/Point.h>
#include <ros/ros.h>
#include <vector>
#include <map>
#include <string>
#include <limits>
#include <algorithm>
#include <Eigen/Dense>

typedef pcl::PointXYZI PointType;

// ==================== my_pcl 核心函数 ====================

/**
 * @brief 类似OpenCV imshow的便捷点云发布函数
 * @param topic_name 发布的ROS话题名
 * @param cloud 要发布的点云
 * @param r,g,b 点云颜色 (0-255)
 * @param frame_id 坐标系ID
 */
void pclshow(const std::string& topic_name, 
             const pcl::PointCloud<PointType>::Ptr& cloud,
             uint8_t r = 255, uint8_t g = 255, uint8_t b = 255,
             const std::string& frame_id = "map");

/**
 * @brief 点云聚类检测函数，处理、聚类、合并点云，并发布彩色聚类结果
 * @param cloud 输入点云
 * @param publish_topic 发布彩色聚类的话题名
 * @param frame_id 坐标系ID
 * @return 返回合并后的聚类点云列表
 */
std::vector<pcl::PointCloud<PointType>::Ptr> processCloudsForStickDetection(
    pcl::PointCloud<PointType>::Ptr cloud,
    const std::string& publish_topic = "/clusters_cloud",
    const std::string& frame_id = "map");

// ==================== ICP模板匹配类 ====================

// ICP配准结果结构
struct ICPMatchResult {
    double fitness_score;
    Eigen::Matrix4f transformation;
    pcl::PointCloud<PointType>::Ptr aligned_template;
    bool converged;
    double x_position;
    double y_position;
    double z_position;
    Eigen::Vector3f normal_vector;  // 法向量
    double radius;                  // 半径
    
    // 构造函数
    ICPMatchResult() : fitness_score(std::numeric_limits<double>::max()), 
                      converged(false), x_position(0), y_position(0), z_position(0),
                      radius(0) {
        aligned_template = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>);
        normal_vector = Eigen::Vector3f::Zero();
        transformation = Eigen::Matrix4f::Identity();
    }
};

// 轨迹点结构
struct TrajectoryPoints {
    geometry_msgs::Point approach_point;   // 接近点
    geometry_msgs::Point center_point;     // 中心点
    geometry_msgs::Point exit_point;       // 离开点
    double total_distance;                 // 总距离
    
    // 构造函数
    TrajectoryPoints() : total_distance(0) {}
};

/**
 * @brief ICP模板匹配类，用于基于点云模板的物体识别
 */
class ICP_Matching {
private:
    std::string template_folder_path_;
    std::vector<std::string> template_files_;
    double icp_fitness_threshold_;
    int max_iterations_;
    double transformation_epsilon_;
    double euclidean_fitness_epsilon_;
    double max_correspondence_distance_;
    int min_cluster_points_;
    
public:
    /**
     * @brief 构造函数
     * @param template_folder_path 模板文件夹路径
     * @param fitness_threshold ICP匹配阈值
     */
    ICP_Matching(const std::string& template_folder_path = "", 
                double fitness_threshold = 0.001);
    
    /**
     * @brief 设置模板文件列表
     * @param template_files 模板文件名列表
     */
    void setTemplateFiles(const std::vector<std::string>& template_files);
    
    /**
     * @brief 设置ICP参数
     */
    void setICPParameters(int max_iterations = 200,
                         double transformation_epsilon = 1e-5,
                         double euclidean_fitness_epsilon = 1e-5,
                         double max_correspondence_distance = 0.5);
    
    /**
     * @brief 设置聚类过滤参数
     */
    void setClusterFilterParameters(int min_cluster_points = 50);
    
    /**
     * @brief 从PCD文件加载模板
     * @param template_file 模板文件名
     * @return 模板点云，加载失败返回nullptr
     */
    pcl::PointCloud<PointType>::Ptr loadTemplate(const std::string& template_file);
    
    /**
     * @brief 执行ICP配准
     * @param template_cloud 模板点云
     * @param target_cluster 目标聚类点云
     * @return ICP匹配结果
     */
    ICPMatchResult performICP(const pcl::PointCloud<PointType>::Ptr& template_cloud,
                             const pcl::PointCloud<PointType>::Ptr& target_cluster);
    
    /**
     * @brief 计算物体属性（法向量和半径）
     * @param object_cloud 物体点云
     * @param result 匹配结果，将更新法向量和半径信息
     */
    void calculateObjectProperties(const pcl::PointCloud<PointType>::Ptr& object_cloud, 
                                  ICPMatchResult& result);
    
    /**
     * @brief 生成穿越轨迹点
     * @param match_result 匹配结果
     * @param approach_distance 接近距离
     * @return 轨迹点结构
     */
    TrajectoryPoints generateTrajectory(const ICPMatchResult& match_result, 
                                       double approach_distance = 0.5);
    
    /**
     * @brief 多模板匹配，在聚类中寻找最佳匹配
     * @param clusters 聚类列表
     * @param visualize_templates 是否可视化模板
     * @return 匹配结果列表
     */
    std::vector<ICPMatchResult> findMatches(
        const std::vector<pcl::PointCloud<PointType>::Ptr>& clusters,
        bool visualize_templates = true);
    
    /**
     * @brief 发布匹配结果的可视化点云
     * @param matches 匹配结果列表
     * @param header ROS消息头
     * @param detected_topic 检测结果话题名
     * @param trajectory_topic 轨迹话题名
     */
    void publishVisualization(const std::vector<ICPMatchResult>& matches,
                             const std_msgs::Header& header,
                             const std::string& detected_topic = "/detected_objects",
                             const std::string& trajectory_topic = "/object_trajectory");
    
    // Getter方法
    double getFitnessThreshold() const { return icp_fitness_threshold_; }
    const std::string& getTemplateFolderPath() const { return template_folder_path_; }
    const std::vector<std::string>& getTemplateFiles() const { return template_files_; }
};

#endif // MY_PCL_H